
#ifndef IALGORITHM_H
#define IALGORITHM_H

#include <BContact>

namespace BWE
{
	inline int MinAxis(const BVector& vector)
	{
		if (vector.x() < vector.y() && vector.x() < vector.z())
			return 0;
		if (vector.y() < vector.x() && vector.y() < vector.z())
			return 1;
		return 2;
	}
	inline int MaxAxis(const BVector& vector)
	{
		if (vector.x() > vector.y() && vector.x() > vector.z())
			return 0;
		if (vector.y() > vector.x() && vector.y() > vector.z())
			return 1;
		return 2;
	}

	inline void SetMax(BVector& vector, const BVector& other)
	{
		if (vector.x() < other.x())	vector.x() = other.x();
		if (vector.y() < other.y())	vector.y() = other.y();
		if (vector.z() < other.z())	vector.z() = other.z();
	}
	inline void SetMin(BVector& vector, const BVector& other)
	{
		if (vector.x() > other.x())	vector.x() = other.x();
		if (vector.y() > other.y())	vector.y() = other.y();
		if (vector.z() > other.z())	vector.z() = other.z();
	}

	inline void CalculateVelocity(const BMatrix& matrix0, const BMatrix& matrix1, BVector& linVel, BVector& angVel)
	{
		linVel = (matrix1.position() - matrix0.position());

		BMatrix dmat = matrix1 * matrix0.inverse();
		BQuater dorn = dmat.rotate();
		dorn.normalize();

		BReal angle = dorn.angle();
		BVector axis = BVector(dorn.x(), dorn.y(), dorn.z());

		BReal len = axis.length2();
		if (len < REAL_EPSILON * REAL_EPSILON)
			axis.set(1, 0, 0);
		else
			axis /= sqrt(len);

		angVel = axis * angle;
	}

	inline BMatrix IntegrateTransform(const BMatrix& curTrans, const BVector& linearVelocity, const BVector& angularVelocity, BReal timeStep)
	{
		BMatrix result = curTrans;
		result.setPosition(curTrans.position() + linearVelocity * timeStep);

		if (angularVelocity.valid())
		{
			BReal angle = 0;
			BReal length2 = angularVelocity.length2();
			if (length2 > REAL_EPSILON)
			{
				angle = sqrt(length2);
			}

			if (angle * timeStep > (PI / 4))
			{
				angle = (PI / 4) / timeStep;
			}

			BVector axis;
			if (angle < BReal(0.000000000001))
			{
				axis = angularVelocity * (BReal(0.5) * timeStep - (timeStep * timeStep * timeStep) * (BReal(0.020833333333)) * angle * angle);
			}
			else
			{
				axis = angularVelocity * (sin(BReal(0.5) * angle * timeStep) / angle);
			}
			BQuater dorn(axis.x(), axis.y(), axis.z(), cos(angle * timeStep * BReal(0.5)));
			BQuater orn0 = curTrans.rotate();
			BQuater predictedOrn = (dorn * orn0).normal();
			result.setRotate(predictedOrn);
		}
		return result;
	}

	struct CastResult
	{
		BReal fraction = REAL_LARGE;
		BReal distance = REAL_LARGE;
		BVector normal;
		BVector point;
	};

	struct SolverData
	{
		BBody*		body = 0;
		BVector		forceVelocity;
		BVector		torqueVelocity;
		BVector		linearVelocity;
		BVector		angularVelocity;
		BVector		pushVelocity;
		BVector		turnVelocity;
		BReal		hitFraction = 1;
	};

	class IAlgorithm : public BThing
	{
	public:
		IAlgorithm();
		~IAlgorithm();

		virtual void caculate() = 0;
		virtual void timeImpact() = 0;

		void swap();

		void setBodyA(BBody* body, BShape* shape, const BMatrix& matrix);
		void setBodyB(BBody* body, BShape* shape, const BMatrix& matrix);

		void addContactPoint(const BVector& pointB, const BVector& normalB, BReal distance, BReal fraction = 1);
		int sortContacts(const BContact& contact);
		void refreshContactPoints();

		BBody*		_bodyA;
		BShape*		_shapeA;
		BMatrix		_matrixA;

		BBody*		_bodyB;
		BShape*		_shapeB;
		BMatrix		_matrixB;
		int			numContact;
		BContact	contacts[4];

	};

	typedef BHolder<IAlgorithm>			IAlgorithmHolder;
	typedef BArray<IAlgorithmHolder>	IAlgorithmHolderArray;
}

#endif
